#include "stm32f10x.h"                  // Device header
#include "PWM.h"
#include "Motor.h"

/*电机引脚定义（自行复制，我懒得弄了……）*/
#define L298N_IN1 GPIO_Pin_5
#define L298N_IN2 GPIO_Pin_0
#define L298N_IN3 GPIO_Pin_10
#define L298N_IN4 GPIO_Pin_15

#define TB6612_AIN1 GPIO_Pin_13
#define TB6612_AIN2 GPIO_Pin_14
#define TB6612_BIN1 GPIO_Pin_11
#define TB6612_BIN2 GPIO_Pin_12

/**
  * @brief  电机初始化
  * @param  无
  * @retval 无
  */
void Motor_Init()
{
	PWM_Init();
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_11|GPIO_Pin_12;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_5|GPIO_Pin_0|GPIO_Pin_10|GPIO_Pin_15|GPIO_Pin_13|GPIO_Pin_14;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
}

/**
  * @brief  设置总速度
  * @param  AllSpeed：速度值，一个指向SetMotor_Speed的结构体指针
  * @retval 无
  */
void Motor_SetAllSpeed(SetMotor_Speed* AllSpeed)
{
	Motor_SetDuty1(AllSpeed);
	Motor_SetDuty2(AllSpeed);
	Motor_SetDuty3(AllSpeed);
	Motor_SetDuty4(AllSpeed);
}

/**
  * @brief  设置电机1的占空比
  * @param  Speed：速度值，一个指向SetMotor_Speed的结构体指针
  * @retval 无
  */
void Motor_SetDuty1(SetMotor_Speed* Speed)
{
	if(Speed->Speed1>100)Speed->Speed1=100;
	if(Speed->Speed2>100)Speed->Speed2=100;
	if(Speed->Speed3>100)Speed->Speed3=100;
	if(Speed->Speed4>100)Speed->Speed4=100;
	
	if(Speed->Speed1<=0)
	{
		GPIO_ResetBits(GPIOB,GPIO_Pin_13);
		GPIO_SetBits(GPIOB,GPIO_Pin_14);
		PWM_SetCompare1(-Speed->Speed1);
	}
	else
	{
		GPIO_SetBits(GPIOB,GPIO_Pin_13);
		GPIO_ResetBits(GPIOB,GPIO_Pin_14);
		PWM_SetCompare1(Speed->Speed1);
	}
}

/**
  * @brief  设置电机2的占空比
  * @param  Speed：速度值，一个指向SetMotor_Speed的结构体指针
  * @retval 无
  */
void Motor_SetDuty2(SetMotor_Speed* Speed)
{	
	if(Speed->Speed1>100)Speed->Speed1=100;
	if(Speed->Speed2>100)Speed->Speed2=100;
	if(Speed->Speed3>100)Speed->Speed3=100;
	if(Speed->Speed4>100)Speed->Speed4=100;
	
	if(Speed->Speed2<=0)
	{
		GPIO_SetBits(GPIOA,GPIO_Pin_11);
		GPIO_ResetBits(GPIOA,GPIO_Pin_12);
		PWM_SetCompare2(-Speed->Speed2);
	}
	else
	{
		GPIO_ResetBits(GPIOA,GPIO_Pin_11);
		GPIO_SetBits(GPIOA,GPIO_Pin_12);
		PWM_SetCompare2(Speed->Speed2);
	}
}

/**
  * @brief  设置电机3的占空比
  * @param  Speed：速度值，一个指向SetMotor_Speed的结构体指针
  * @retval 无
  */
void Motor_SetDuty3(SetMotor_Speed* Speed)
{
	if(Speed->Speed1>100)Speed->Speed1=100;
	if(Speed->Speed2>100)Speed->Speed2=100;
	if(Speed->Speed3>100)Speed->Speed3=100;
	if(Speed->Speed4>100)Speed->Speed4=100;
	
	if(Speed->Speed3<=0)
	{
		GPIO_SetBits(GPIOB,GPIO_Pin_5);
		GPIO_ResetBits(GPIOB,GPIO_Pin_0);
		PWM_SetCompare3(-Speed->Speed3);
	}
	else
	{
		GPIO_ResetBits(GPIOB,GPIO_Pin_5);
		GPIO_SetBits(GPIOB,GPIO_Pin_0);
		PWM_SetCompare3(Speed->Speed3);
	}
}

/**
  * @brief  设置电机4的占空比
  * @param  Speed：速度值，一个指向SetMotor_Speed的结构体指针
  * @retval 无
  */
void Motor_SetDuty4(SetMotor_Speed* Speed)
{
	if(Speed->Speed1>100)Speed->Speed1=100;
	if(Speed->Speed2>100)Speed->Speed2=100;
	if(Speed->Speed3>100)Speed->Speed3=100;
	if(Speed->Speed4>100)Speed->Speed4=100;
	
	if(Speed->Speed4<=0)
	{
		GPIO_SetBits(GPIOB,GPIO_Pin_10);
		GPIO_ResetBits(GPIOB,GPIO_Pin_15);
		PWM_SetCompare4(-Speed->Speed4);
	}
	else
	{
		GPIO_ResetBits(GPIOB,GPIO_Pin_10);
		GPIO_SetBits(GPIOB,GPIO_Pin_15);
		PWM_SetCompare4(Speed->Speed4);
	}
}
